#include "FaceTracker.hpp"
#include "HiVisualiser.hpp"

#include <iostream>
#include "HIGeom-inl.hpp"

using namespace cv;
using namespace std;
using namespace hi;

FaceTracker::FaceTracker( Ptr< AbstractProbabilityTransformer > probTrans ) : probTrans(probTrans) { }

void FaceTracker::track( cv::Mat& nextImage, cv::Rect& trackingRect ) {
	
	Rect faceScanRect = getFaceScanRect( nextImage.size(), trackingRect );
	
	Mat faceScanImg( nextImage, faceScanRect );
	
	//Compute HSV for backprojection
	cvtColor(faceScanImg, imgFaceHSV, CV_BGR2HSV);

	//Compute backprojection
	probTrans->getBackProjection( imgFaceHSV, faceBackProj );
	
//	//VISUALISATION
//	HiVisualiser::windowMatrixMap["face bp"] = faceBackProj;
//	HiVisualiser::refreshWindow("face bp");

	//change frame of reference
	trackingRect.x -= faceScanRect.x;
	trackingRect.y -= faceScanRect.y;
	
	meanShift( faceBackProj, trackingRect, TermCriteria( TermCriteria::MAX_ITER + TermCriteria::EPS , 3, 0.01) );
	
	//revert frame of reference
	trackingRect.x += faceScanRect.x;
	trackingRect.y += faceScanRect.y;
}


Rect FaceTracker::getFaceScanRect( const cv::Size& outerFrame, const cv::Rect& trackingRect ) {
	int rectOriginX = trackingRect.x - trackingRect.width;
	rectOriginX = ( rectOriginX < 0 ) ? 0 : rectOriginX;
	
	int rectOriginY = trackingRect.y - trackingRect.height;
	rectOriginY = ( rectOriginY < 0 ) ? 0 : rectOriginY;
	
	int rectWidth = trackingRect.width * 3;
	rectWidth = ( rectWidth < outerFrame.width - rectOriginX ) ? rectWidth : outerFrame.width - rectOriginX;
	
	int rectHeight = trackingRect.height * 3;
	rectHeight = ( rectHeight < outerFrame.height - rectOriginY ) ? rectHeight : outerFrame.height - rectOriginY;
	
	return Rect( rectOriginX, rectOriginY, rectWidth, rectHeight );
}
